package org.scalpel.mainApps;

import org.scalpel.locodo.MotorInterface;
import org.scalpel.locodo.PID;


/**
 * 
 * @author Jerome Monceaux
 *
 * Simulate a motor by incrementing an virtual encoder value according to the motor speed 
 */
public class GenericVirtualMotor implements MotorInterface {

  private double fSpeed;
  private double fEncVal;
  //private double fMaxAcc;
  private PID fPid;  
  @SuppressWarnings("unused")
  private double fMaxSpeed;
  double fRealSpeed;
  double fMaxRealSpeedinTickPerSecond;
  double fLastPidOuput;

  
  public GenericVirtualMotor( double maxRealSpeedinTickPerSecond) {
    fSpeed = 0.f;
    fEncVal = 0.f;
    //fMaxAcc = 0.7f; // encoder ticks per cycle
    fRealSpeed = 0.;
    fMaxSpeed = 0.5f;
    fMaxRealSpeedinTickPerSecond = maxRealSpeedinTickPerSecond;
    fLastPidOuput = 0.;
    
    //Simulation of PID
    
    fPid = new PID();
    fPid.P = 1.;
    fPid.I = 0.;
    fPid.D = 0.;

  }

  
  @Override
  public int getEncoderValue() {
    return (int) fEncVal;
  }

  
  @Override
  public void setSpeed(double speed) {
    fSpeed = speed;
  }

  
  @Override
  public void update() {

    fPid.setCurrValue(fSpeed - fRealSpeed, 1);
    double s = /*fMaxAcc * */fPid.update(); // Pid clip if output > 1 or output < -1
    
    fLastPidOuput += s;
    // Warning do not simulate saturation speed like this
    // The angle control is not effective at max speed...
    //if( fSpeed > fMaxSpeed ) fSpeed = fMaxSpeed; 
    //if( fSpeed < -fMaxSpeed ) fSpeed = -fMaxSpeed; 

    double lastEncVal = fEncVal; 
    fEncVal += 100. * fLastPidOuput;
    
    fRealSpeed = (fEncVal - lastEncVal) / (fMaxRealSpeedinTickPerSecond/50.);

  }


  public void setPid(double P, double I, double D)
  {  
    fPid.P = P;
    fPid.I = I;
    fPid.D = D;
  }

}